import random
import numpy as np
from typing import List, Tuple, Union

from abc import ABC, abstractmethod



class Position(object):
    """3D坐标位置"""

    def __init__(self, x:float, y:float, z:float):
        self._coords = np.array([x, y, z], dtype=float)
    
    @property
    def x(self): return self._coords[0]
    
    @property
    def y(self): return self._coords[1]
    
    @property
    def z(self): return self._coords[2]

    @property
    def array(self): return self._coords

    def distance_to(self, other: 'Position') -> float:
        """计算到另一个位置的欧几里得距离"""
        return np.linalg.norm(other._coords - self._coords)

    def vector_to(self, other: 'Position') -> np.ndarray:
        """计算指向另一个位置的向量 (other - self)"""
        return other._coords - self._coords
    
    def move(self, vector: np.ndarray):
        """根据给定的向量更新当前位置"""
        self._coords += vector

    def clone(self):
        """返回当前位置的副本"""
        return Position(self.x, self.y, self.z)
    
    def __repr__(self):
        return f'Postion({self.x:.2f},{self.y:.2f},{self.z:.2f})'




class SpeedModel(ABC):
    """速度模型基类"""
    
    @abstractmethod
    def get_speed(self, time):
        """返回当前时间的速度"""
        pass

class ConstantSpeed(SpeedModel):
    """定速模型"""
    
    def __init__(self, speed:float):
        self.speed = speed
        
    def get_speed(self, time):
        return self.speed

class LinearAccelerationSpeed(SpeedModel):
    """线性加速模型"""
    
    def __init__(self, initial_speed:float, acceleration:float, max_speed:float):
        self.v0 = initial_speed
        self.acc = acceleration
        self.v_max = max_speed
        self.start_time = 0
        
    def get_speed(self, time):
        dt = time - self.start_time
        v = self.v0 + self.acc * dt
        return min(v, self.v_max)
        
class RandomSpeed(SpeedModel):
    """随机速度模型，在范围内随机变化"""

    def __init__(self, min_speed: float, max_speed:float):
        self.min_speed = min_speed
        self.max_speed = max_speed
        
    def get_speed(self, time):
        return random.uniform(self.min_speed, self.max_speed)




"""
@defgroup Mobility Model 
"""
class MobilityModel(ABC):

    def __init__(self, speed_model: SpeedModel):
        self.speed_model = speed_model

    @abstractmethod
    def step(self, node, dt: float):
        """
        计算并更新节点在dt时间后的新位置
        @node: 节点实例
        @dt: 时间步长
        """
        
class HoverMobility(MobilityModel):
    """悬停模式"""
    
    def step(self, node, dt:float):
        pass

class LinearMobility(MobilityModel):
    """线性移动"""

    def __init__(self, speed_model:SpeedModel, destination:Position):
        super().__init__(speed_model)
        self.destination = destination
        
    def step(self, node, dt:float):
        
        # 计算距离和向量
        dist = node.position.distance_to(self.destination)
        
        if dist < 0.01:
            return
        
        # 获取方向向量
        vector = node.position.vector_to(self.destination)
        
        # 归一化
        norm_direction = vector / dist
        
        speed = self.speed_model.get_speed(node.env.now)
        move_dist = min(speed * dt, dist)
        
        # 更新节点位置
        node.position.move(norm_direction * move_dist)
        
class RandomWaypointMobility(MobilityModel):
    """在边界内随机移动"""

    def __init__(self, speed_model:SpeedModel, x_range: Tuple[float, float], y_range: Tuple[float, float], z_range: Tuple[float, float]):
        super().__init__(speed_model)
        self.ranges = (x_range, y_range, z_range)
        self.target: Union[Position, None] = None

    def _pick_new_target(self):
        x = random.uniform(*self.ranges[0])
        y = random.uniform(*self.ranges[1])
        z = random.uniform(*self.ranges[2])
        self.target = Position(x, y, z)

    def step(self, node, dt:float):
        if self.target is None:
            self._pick_new_target()

        dist = node.position.distance_to(self.target)
        
        if dist < 0.1:
            self._pick_new_target()
            return
        
        vector = node.position.vector_to(self.target)
        norm_dir = vector / dist
        
        speed = self.speed_model.get_speed(node.env.now)
        move_dist = min(speed * dt, dist)
        
        node.position.move(norm_dir * move_dist)
        
class TrajectoryMobility(MobilityModel):
    """轨迹移动"""

    def __init__(self, speed_model:SpeedModel, waypoints: List[Position], loop=False):
        super().__init__(speed_model)
        self.waypoints = waypoints
        self.current_idx = 0
        self.loop = loop

    def step(self, node, dt:float):
        if self.current_idx >= len(self.waypoints):
            return

        target = self.waypoints[self.current_idx]
        dist = node.position.distance_to(target)
        vector = node.position.vector_to(target)
        
        speed = self.speed_model.get_speed(node.env.now)
        
        if speed * dt >= dist:
            # 这一步可以到达或越过目标，直接设为目标位置
            # 这里需要小心，不能直接赋值引用，要设置坐标，或者让位置移动过去
            node.position.move(vector)
            self.current_idx += 1
            if self.loop and self.current_idx >= len(self.waypoints):
                self.current_idx = 0
        else:
            norm_dir = vector / dist
            node.position.move(norm_dir * (speed * dt))
            